package org.gatech.linalg;

import java.util.ArrayList;
import java.util.Arrays;

public class Matrix {

	private double[][] vals;
	private int rows;
	private int cols;
	public final static int QR_ITERATION = 0, JACOBI_ITERATION = 1;

	public Matrix(double[][] vals) throws DimensionMismatchException {
		this.vals = vals;
		int colsize = vals[0].length;
		for (int i = 0; i < vals.length; i++)
			if (vals[i].length != colsize)
				throw new DimensionMismatchException();
		rows = vals.length;
		cols = vals[0].length;
	}

	public Matrix(int[][] vals) throws DimensionMismatchException {
		this.vals = new double[vals.length][vals[0].length];
		for (int i = 0; i < vals.length; i++)
			for (int j = 0; j < vals[i].length; j++)
				this.vals[i][j] = vals[i][j];
		int colsize = vals[0].length;
		for (int i = 0; i < vals.length; i++)
			if (vals[i].length != colsize)
				throw new DimensionMismatchException();
		rows = vals.length;
		cols = vals[0].length;
	}

	public Matrix(Matrix m) {
		vals = new double[m.rows][m.cols];
		for (int i = 0; i < m.rows; i++)
			for (int j = 0; j < m.cols; j++)
				vals[i][j] = m.vals[i][j];
		rows = vals.length;
		cols = vals[0].length;
	}

	public static Matrix transpose(Matrix m) {
		double[][] newVals = new double[m.cols][m.rows];
		for (int i = 0; i < m.rows; i++)
			for (int j = 0; j < m.cols; j++)
				newVals[j][i] = m.vals[i][j];
		try {
			return new Matrix(newVals);
		} catch (DimensionMismatchException e) {
			return null;
		}
	}

	public static Matrix inverse(Matrix m) throws NonInvertibleException,
			DimensionMismatchException {
		if (m.rows != m.cols)
			throw new DimensionMismatchException();
		Matrix aug = augment(m, makeIdentity(m.rows));
		Matrix reduced = rref(aug);
		double[][] leftVals = new double[m.rows][m.cols], rightVals = new double[m.rows][m.cols];
		for (int i = 0; i < reduced.rows; i++) {
			for (int j = 0; j < reduced.cols; j++) {
				if (j < m.cols)
					leftVals[i][j] = reduced.vals[i][j];
				else
					rightVals[i][j % m.cols] = reduced.vals[i][j];
			}
		}
		Matrix L = new Matrix(leftVals), R = new Matrix(rightVals);
		if (!L.equals(makeIdentity(m.rows)))
			throw new NonInvertibleException();
		return R;
	}

	public static double determinant(Matrix m)
			throws DimensionMismatchException {
		if (!m.isSquare())
			throw new DimensionMismatchException();
		Matrix[] LU = LUDecomposition(m);
		double determinant = 1;
		for (int i = 0; i < LU[1].cols; i++)
			determinant *= LU[1].vals[i][i];
		return (Math.abs(determinant - (int) determinant) < 0.00001 ? (int) (determinant)
				: Math.abs(determinant - (int) determinant - 1) < 0.00001 ? (int) determinant + 1
						: determinant);
	}

	public static Matrix ref(Matrix m) {
		double[][] M = new double[m.rows][m.cols];
		for (int i = 0; i < m.rows; i++)
			for (int j = 0; j < m.cols; j++)
				M[i][j] = m.vals[i][j];
		int nr = m.rows;
		int nc = m.cols;

		for (int r = 0; r < nr; r++) {
			boolean allZero = true;
			for (int c = 0; c < nc; c++) {
				if (!equalsWithEp(M[r][c], 0.0, 0.00000001)) {
					allZero = false;
					break;
				}
			}
			if (allZero) {
				double[] temp = M[r];
				M[r] = M[nr];
				M[nr] = temp;
				nr--;
			}
		}

		int r = 0, c = 0;
		while (r < nr && c < nc) {
			while (equalsWithEp(M[r][c], 0.0, 0.00000001)) {
				boolean swapped = false;
				for (int i = r + 1; i < nr; i++) {
					if (!equalsWithEp(M[i][c], 0.0, 0.00000001) && !swapped) {
						swapped = true;
						double[] temp = M[r];
						M[r] = M[i];
						M[i] = temp;
					}
				}
				if (!swapped) {
					c++;
					if (c >= nc)
						break;
				}
			}

			for (int i = r + 1; i < nr; i++) {
				double x = -1 * M[i][c] / M[r][c];
				for (int j = c; j < nc; j++) {
					M[i][j] += M[r][j] * x;
				}
			}
			r++;
			c++;
		}

		try {
			return new Matrix(M);
		} catch (DimensionMismatchException e) {
			return null;
		}
	}

	public static Matrix rref(Matrix m) {
		double[][] M = new double[m.rows][m.cols];
		for (int i = 0; i < m.rows; i++)
			for (int j = 0; j < m.cols; j++)
				M[i][j] = m.vals[i][j];
		int nr = m.rows;
		int nc = m.cols;

		for (int r = 0; r < nr; r++) {
			boolean allZero = true;
			for (int c = 0; c < nc; c++) {
				if (!equalsWithEp(M[r][c], 0.0, 0.00000001)) {
					allZero = false;
					break;
				}
			}
			if (allZero) {
				double[] temp = M[r];
				M[r] = M[nr];
				M[nr] = temp;
				nr--;
			}
		}

		int r = 0, c = 0;
		while (r < nr && c < nc) {
			while (equalsWithEp(M[r][c], 0.0, 0.00000001)) {
				boolean swapped = false;
				for (int i = r + 1; i < nr; i++) {
					if (!equalsWithEp(M[i][c], 0.0, 0.00000001) && !swapped) {
						swapped = true;
						double[] temp = M[r];
						M[r] = M[i];
						M[i] = temp;
					}
				}
				if (!swapped) {
					c++;
					if (c >= nc)
						break;
				}
			}

			if (r < nr && c < nc) {
				double divisor = M[r][c];
				for (int i = 0; i < m.cols; i++)
					M[r][i] = M[r][i] / divisor;

				for (int i = 0; i < nr; i++) {
					if (i != r) {
						double x = -1 * M[i][c] / M[r][c];
						for (int j = c; j < nc; j++) {
							M[i][j] += M[r][j] * x;
						}
					}
				}
			}
			r++;
			c++;
		}

		try {
			return new Matrix(M);
		} catch (DimensionMismatchException e) {
			return null;
		}
	}

	public static double[] eig(Matrix m) throws DimensionMismatchException {
		return eig(m, 0);
	}

	public static double[] eig(Matrix m, int type)
			throws DimensionMismatchException {
		return eig(m, type, 1e-9);
	}

	public static double[] eig(Matrix m, double precision)
			throws DimensionMismatchException {
		return eig(m, 0, precision);
	}

	public static double[] eig(Matrix m, int type, double precision)
			throws DimensionMismatchException {
		if (m.cols > 2) {
			switch (type) {
			case QR_ITERATION:
				// return QRIteration(m, precision);
			case JACOBI_ITERATION:
				Matrix eigs = jacobiIteration(m, precision)[0];
				double[] values = new double[eigs.rows];
				for (int i = 0; i < values.length; i++)
					values[i] = eigs.getVals()[i][i];
				return values;
			default:
				return null;
			}
		} else {
			double a = 1;
			double b = m.vals[0][0] + m.vals[1][1];
			double c = m.vals[0][0] * m.vals[1][1] - m.vals[0][1]
					* m.vals[1][0];

			double[] eigs = new double[2];

			eigs[0] = -1 * (-1 * b + Math.sqrt(Math.pow(b, 2) - 4 * a * c))
					/ (2 * a);
			eigs[1] = -1 * (-1 * b - Math.sqrt(Math.pow(b, 2) - 4 * a * c))
					/ (2 * a);

			return eigs;
		}
	}

	public static Matrix[] QRIteration(Matrix m, double precision)
			throws DimensionMismatchException {
		Matrix reduced = new Matrix(m);
		Matrix eigenvectors = makeIdentity(m.rows);
		double[] eigs = new double[reduced.rows];
		while (true) {
			double maxError = 0;
			Matrix[] QR = householderDecomp(reduced);
//			System.out.println(reduced.vals[0][0]);
			reduced = product(QR[1], QR[0]);
//			System.out.println(QR[0].vals[0][0]);
//			System.out.println(QR[1].vals[0][0]);
//			try {
//				Thread.sleep(1000);
//			} catch(Exception e) { }
			eigenvectors = product(eigenvectors, QR[0]);
			for (int i = 0; i < reduced.rows; i++) {
				double difference = Math.abs(reduced.vals[i][i] - eigs[i]);
				maxError = maxError + difference;
				eigs[i] = reduced.vals[i][i];
			}
			if (maxError < precision)
				break;
			else {
				System.out.println(maxError);
			}
		}
		double[][] eigsArr = new double[eigs.length][eigs.length];
		for (int i = 0; i < eigs.length; i++)
			eigsArr[i][i] = eigs[i];
		Matrix[] results = new Matrix[2];
		results[0] = eigenvectors;
		results[1] = new Matrix(eigsArr);
		return results;
	}

	// Sums the squares of the off-diagonal elements in a matrix. Used to
	// test the precision of the Jacobi algorithm so it has a stopping point.
	public static double getOffElement(Matrix mat) {
		double[][] m = new Matrix(mat).getVals();
		double result = 0.;
		for (int i = 0; i < m.length; i++)
			for (int j = i + 1; j < m[i].length; j++)
				result += Math.pow(m[i][j], 2);
		return (2 * result);
	}

	// Gets the max element not on the diagonal of a matrix.
	public static double getMaxOffElement(Matrix mat) {
		double[][] m = new Matrix(mat).getVals();
		double max = 0;
		for (int i = 0; i < m.length; i++)
			for (int j = i + 1; j < m[i].length; j++)
				if (Math.abs(m[i][j]) > max) {
					max = Math.abs(m[i][j]);
				}
		return max;
	}

	// Gets the max element not on the diagonal of a matrix.
	public static int[] getMaxOffElementIndex(Matrix mat) {
		double[][] m = new Matrix(mat).getVals();
		int[] coords = { 0, 1 };
		for (int i = 0; i < m.length; i++)
			for (int j = i + 1; j < m[i].length; j++)
				if (Math.abs(m[i][j]) > Math.abs(m[coords[0]][coords[1]])) {
					coords[0] = i;
					coords[1] = j;
				}
		return coords;
	}

	// Is what it is. The Jacobi algorithm for finding eigenvalues and
	// eigenvectors.
	public static Matrix[] jacobiIteration(Matrix m, double precision)
			throws DimensionMismatchException {
		Matrix reduced = new Matrix(m);
		int counter = 0;
		Matrix eigenvectors = makeIdentity(reduced.rows);
		double currentOff = Matrix.getOffElement(reduced);
		while (currentOff > precision) {
			for (int i = 0; i < reduced.rows; i++)
				for (int j = 0; j < reduced.rows; j++)
					if (reduced.getVals()[i][j] < .05)
						reduced.getVals()[i][j] = 0;

			System.out.println(Matrix.getOffElement(reduced));

			// System.err.println("Jacobi MARK 1");

			// Get the indices of the maximum offdiagonal element.
			int[] indices = getMaxOffElementIndex(reduced);

			// Calculate the eigenvalues for the given position
			double a = reduced.getVals()[indices[0]][indices[0]];
			double b = reduced.getVals()[indices[0]][indices[1]];
			double d = reduced.getVals()[indices[1]][indices[1]];

			// System.err.println("Jacobi MARK 2");

			double eig = (a + d) / 2
					+ Math.sqrt(b * b + ((a - d) / 2) * ((a - d) / 2));

			// calculate eVnorm
			double eVnorm = Math.sqrt(b * b + ((a - eig) * (a - eig)));

			// System.err.println("Jacobi MARK 3");

			// Make the givens Matrix
			double[][] givensArr = makeIdentity(reduced.rows).getVals();
			givensArr[indices[0]][indices[0]] = b / eVnorm;
			givensArr[indices[0]][indices[1]] = (a - eig) / eVnorm;
			givensArr[indices[1]][indices[0]] = -1
					* (givensArr[indices[0]][indices[1]]);
			givensArr[indices[1]][indices[1]] = (givensArr[indices[0]][indices[0]]);
			Matrix givens = new Matrix(givensArr);

			// System.err.println("Jacobi MARK 4");
			eigenvectors = product(eigenvectors, givens);

			// Multiply givens by reduced to get A(k+1)
			reduced = product(givensTranspose(givens, indices), reduced);
			reduced = product(reduced, givens);

			// System.err.println("Jacobi MARK 5");

			currentOff = Matrix.getMaxOffElement(reduced);
			// if(thisOff == currentOff)
			// break;
			// else
			// currentOff = thisOff;

			counter++;
		}
		Matrix[] results = { reduced, eigenvectors };
		return results;
	}

	public static Matrix rightGivensProduct(Matrix m1, Matrix g, int[] indices)
			throws DimensionMismatchException {
		Matrix result = new Matrix(m1);
		int i = indices[0];
		int j = indices[1];
		if (m1.cols != g.rows)
			throw new DimensionMismatchException();
		for (int k = 0; k < m1.cols; k++) {
			for (int f = 0; f < m1.cols; f++) {
				result.getVals()[i][k] += m1.getVals()[i][k]
						* g.getVals()[f][i];
				result.getVals()[j][k] += m1.getVals()[i][k]
						* g.getVals()[f][j];
				System.out.println("stuck");
			}
		}
		System.out.println("leaving");
		return result;
	}

	public static Matrix leftGivensProduct(Matrix g, Matrix m1, int[] indices)
			throws DimensionMismatchException {
		Matrix result = new Matrix(m1);
		int i = indices[0];
		int j = indices[1];
		if (m1.cols != g.rows)
			throw new DimensionMismatchException();
		for (int k = 0; k < m1.cols; k++) {
			for (int f = 0; f < m1.cols; f++) {
				result.getVals()[i][k] += m1.getVals()[i][k]
						* g.getVals()[f][i];
				result.getVals()[j][k] += m1.getVals()[i][k]
						* g.getVals()[f][j];
			}
		}
		return result;
	}

	public static Matrix givensTranspose(Matrix m, int[] indeces)
			throws DimensionMismatchException {
		Matrix transposed = new Matrix(m);
		double temp = transposed.getVals()[indeces[0]][indeces[1]];
		transposed.getVals()[indeces[0]][indeces[1]] = transposed.getVals()[indeces[1]][indeces[0]];
		transposed.getVals()[indeces[1]][indeces[0]] = temp;
		return transposed;
	}

	public static Matrix[] householderDecomp(Matrix m)
			throws DimensionMismatchException {
		if (!m.isSquare())
			throw new DimensionMismatchException();

		Matrix[] results = new Matrix[2];
		results[0] = makeIdentity(m.cols);
		results[1] = new Matrix(m);

//		System.err.println("householder check 1");

		for (int i = 0; i < m.cols; i++) {
			double[][] colVals = new double[m.cols - i][1];
			for (int j = m.rows - i-1; j >= 0; j--) {
				colVals[j][0] = m.vals[j + i][i];
			}
			Matrix columnMatrix = new Matrix(colVals);
			columnMatrix.vals[0][0] = columnMatrix.vals[0][0]
					+ ((columnMatrix.vals[0][0] >= 0 ? 1 : -1) * vectorMag(columnMatrix));
			Matrix householderTemp = generateHouseholder(columnMatrix);
			Matrix householder = makeIdentity(m.cols);
			for (int j = i; j < householder.rows; j++) {
				for (int k = i; k < householder.cols; k++) {
					householder.vals[j][k] = householderTemp.vals[j - i][k - i];
				}
			}
			results[0] = product(results[0], householder);
			results[1] = product(transpose(householder), results[1]);
//			System.err.println("householder - at " + i + " out of "
//					+ results[1].cols);
		}

//		System.err.println("householder check 2");

		// double[][] firstNewVals = new
		// double[results[0].rows][results[0].cols];
		// for (int k = 0; k < results[0].rows; k++) {
		// for (int l = 0; l < results[0].cols; l++) {
		// firstNewVals[k][l] = results[0].getVals()[k][l] * -1;
		// }
		// }
		// results[0] = new Matrix(firstNewVals);
		//
		// double[][] secondNewVals = new
		// double[results[1].rows][results[1].cols];
		// for (int k = 0; k < results[1].rows; k++) {
		// for (int l = 0; l < results[1].cols; l++) {
		// secondNewVals[k][l] = results[1].getVals()[k][l] * -1;
		// }
		// }
		// results[1] = new Matrix(secondNewVals);

		return results;
	}

	public static Matrix generateHouseholder(Matrix u) {
		try {
			// I don't think the u defined here is correct. The unit vector is
			// supposed to be the first vector
			// of the Householder matrix + or - the vector magnitude of that
			// vector times e(1). To basically
			// the
			Matrix result = product(u, transpose(u));
			result = scalarMult(result, 2/Math.pow(vectorMag(u),2));
			result = difference(makeIdentity(u.rows), result);
			return result;
		} catch (Exception e) {
			return null;
		}
	}

	public static Matrix eigenVectors(Matrix m, double percision)
			throws DimensionMismatchException {
		Matrix reduced = new Matrix(m);
		double[] eigs = new double[reduced.rows];
		Matrix[] QR = null;

		while (true) {
			double maxError = 0;
			QR = householderDecomp(reduced);
			reduced = product(QR[1], QR[0]);
			for (int i = 0; i < reduced.rows; i++) {
				double difference = Math.abs(reduced.vals[i][i] - eigs[i]);
				if (maxError < difference)
					maxError = difference;
				eigs[i] = reduced.vals[i][i];
			}
			if (maxError < percision)
				break;
		}

		return QR[0];
	}

	public static Matrix eigenVectorsFromValues(Matrix m, double percision)
			throws DimensionMismatchException {
		double[] eigenValues = eig(m, percision);

		double[][] eigenVectors = new double[m.rows][m.cols];

		for (int i = 0; i < m.rows; i++) {
			double currentVal = 0;
			try {
				currentVal = eigenValues[i];
			} catch (ArrayIndexOutOfBoundsException e) {
			}

			double[][] matrixValues = new Matrix(m).getVals();

			for (int j = 0; j < matrixValues.length; j++) {
				matrixValues[j][j] -= currentVal;
			}

			Matrix toSolve = new Matrix(matrixValues);
			double[][] zerosArr = new double[toSolve.rows][1];
			for (int j = 0; j < zerosArr.length; j++) {
				zerosArr[j][0] = 0.0;
			}
			Matrix zeros = new Matrix(zerosArr);

			Matrix solved = null;
			try {
				solved = solve(toSolve, zeros);
			} catch (NonSolvableException e) {
				System.err.println("SHEEEEEEEEIT!");
				System.exit(7);
			}

			for (int j = 0; j < eigenVectors.length; j++) {
				eigenVectors[j][i] = solved.getVals()[j][0];
			}
		}

		return new Matrix(eigenVectors);
	}

	public static double vectorMag(Matrix m) {
		Matrix mDup = new Matrix(m);
		if (mDup.cols != 1)
			mDup = transpose(mDup);
		double sum = 0;
		int i = 0;
		for (; i < mDup.rows; i++) {
			sum += Math.pow(mDup.vals[i][0], 2);
		}
		return Math.sqrt(sum);
	}

	public static Matrix[] LUDecomposition(Matrix m)
			throws DimensionMismatchException {
		if (m.rows != m.cols)
			throw new DimensionMismatchException();
		double[][] M = new double[m.rows][m.cols];
		for (int i = 0; i < m.rows; i++)
			for (int j = 0; j < m.cols; j++)
				M[i][j] = m.vals[i][j];
		double[][] L = new double[m.rows][m.cols];
		for (int i = 0; i < m.rows; i++)
			L[i][i] = 1;
		int nr = m.rows;
		int nc = m.cols;

		for (int r = 0; r < nr; r++) {
			boolean allZero = true;
			for (int c = 0; c < nc; c++) {
				if (!equalsWithEp(M[r][c], 0.0, 0.00000001)) {
					allZero = false;
					break;
				}
			}
			if (allZero) {
				double[] temp = M[r];
				M[r] = M[nr];
				M[nr] = temp;
				nr--;
			}
		}

		int r = 0, c = 0;
		while (r < nr && c < nc) {
			while (equalsWithEp(M[r][c], 0.0, 0.00000001)) {
				boolean swapped = false;
				for (int i = r + 1; i < nr; i++) {
					if (!equalsWithEp(M[i][c], 0.0, 0.00000001) && !swapped) {
						swapped = true;
						double[] temp = M[r];
						M[r] = M[i];
						M[i] = temp;
					}
				}
				if (!swapped) {
					c++;
					if (c >= nc)
						break;
				}
			}

			for (int i = r + 1; i < nr; i++) {
				double x = -1 * M[i][c] / M[r][c];
				L[i][c] = -1 * x;
				for (int j = c; j < nc; j++) {
					M[i][j] += M[r][j] * x;
				}
			}
			r++;
			c++;
		}

		try {
			Matrix[] results = new Matrix[2];
			results[0] = new Matrix(L);
			results[1] = new Matrix(M);
			return results;
		} catch (DimensionMismatchException e) {
			return null;
		}
	}

	public static Matrix sum(Matrix a, Matrix b)
			throws DimensionMismatchException {
		if (a.rows != b.rows || a.cols != b.cols)
			throw new DimensionMismatchException();
		double[][] M = new double[a.rows][a.cols];
		for (int i = 0; i < a.rows; i++)
			for (int j = 0; j < a.cols; j++)
				M[i][j] = a.vals[i][j];
		for (int i = 0; i < a.rows; i++)
			for (int j = 0; j < a.cols; j++)
				M[i][j] = a.vals[i][j] + b.vals[i][j];
		try {
			return new Matrix(M);
		} catch (DimensionMismatchException e) {
			return null;
		}
	}

	public static Matrix difference(Matrix a, Matrix b)
			throws DimensionMismatchException {
		if (a.rows != b.rows || a.cols != b.cols)
			throw new DimensionMismatchException();
		double[][] M = new double[a.rows][a.cols];
		for (int i = 0; i < a.rows; i++)
			for (int j = 0; j < a.cols; j++)
				M[i][j] = a.vals[i][j];
		for (int i = 0; i < a.rows; i++)
			for (int j = 0; j < a.cols; j++)
				M[i][j] = a.vals[i][j] - b.vals[i][j];
		try {
			return new Matrix(M);
		} catch (DimensionMismatchException e) {
			return null;
		}
	}

	public static double dot(Matrix a, Matrix b)
			throws DimensionMismatchException {
		Matrix aDup = new Matrix(a);
		Matrix bDup = new Matrix(b);
		if (aDup.rows != 1)
			aDup = transpose(aDup);
		if (bDup.cols != 1)
			bDup = transpose(bDup);
		if (aDup.rows != 1 || bDup.cols != 1 || aDup.cols != bDup.rows)
			throw new DimensionMismatchException();
		return product(aDup, bDup).vals[0][0];
	}

	public static Matrix product(Matrix a, Matrix b)
			throws DimensionMismatchException {
		if (a.cols != b.rows)
			throw new DimensionMismatchException();
		double[][] M = new double[a.rows][b.cols];
		for (int i = 0; i < a.rows; i++) {
			for (int j = 0; j < b.cols; j++) {
				double sum = 0;
				for (int k = 0; k < a.cols; k++)
					sum += a.vals[i][k] * b.vals[k][j];
				M[i][j] = sum;
			}
		}
		try {
			return new Matrix(M);
		} catch (DimensionMismatchException e) {
			return null;
		}
	}

	public static Matrix makeIdentity(int size) {
		double[][] M = new double[size][size];
		for (int i = 0; i < size; i++)
			M[i][i] = 1;
		try {
			return new Matrix(M);
		} catch (DimensionMismatchException e) {
			return null;
		}
	}

	public static Matrix crossProduct(Matrix m1, Matrix m2)
			throws DimensionMismatchException {
		Matrix aDup = new Matrix(m1);
		Matrix bDup = new Matrix(m2);
		if (aDup.rows != 1)
			aDup = transpose(aDup);
		if (bDup.cols != 1)
			bDup = transpose(aDup);
		if (aDup.cols != 1 || bDup.cols != 1 || aDup.rows != 3
				|| bDup.rows != 3)
			throw new DimensionMismatchException();
		double[][] cross = new double[3][1];
		cross[0][0] = aDup.vals[1][0] * bDup.vals[2][0] - bDup.vals[1][0]
				* aDup.vals[2][0];
		cross[1][0] = -1
				* (aDup.vals[0][0] * bDup.vals[2][0] - bDup.vals[0][0]
						* aDup.vals[2][0]);
		cross[2][0] = aDup.vals[0][0] * bDup.vals[1][0] - bDup.vals[0][0]
				* aDup.vals[1][0];
		return new Matrix(cross);
	}

	public static Matrix augment(Matrix m1, Matrix m2)
			throws DimensionMismatchException {
		if (m1.rows != m2.rows)
			throw new DimensionMismatchException();
		double[][] M = new double[m1.rows][m1.cols + m2.cols];
		for (int i = 0; i < m1.rows; i++) {
			for (int j = 0; j < m1.cols; j++) {
				M[i][j] = m1.vals[i][j];
			}
		}
		for (int i = 0; i < m1.rows; i++) {
			for (int j = m1.cols; j < m1.cols + m2.cols; j++) {
				M[i][j] = m2.vals[i][j - m1.cols];
			}
		}
		return new Matrix(M);
	}

	public static Matrix solve(Matrix A, Matrix b)
			throws DimensionMismatchException, NonSolvableException {
		if (b.cols != 1)
			b = Matrix.transpose(b);
		if (b.cols != 1 || b.rows != A.rows)
			throw new DimensionMismatchException();
		Matrix augmented = Matrix.augment(A, b);
		augmented = rref(augmented);
		int size = augmented.cols - 1;
		if (augmented.rows < size)
			throw new NonSolvableException();
		double[][] compareVals = new double[size][size];
		for (int i = 0; i < size; i++)
			for (int j = 0; j < size; j++)
				compareVals[i][j] = augmented.vals[i][j];
		Matrix test = new Matrix(compareVals);
		if (!test.equals(makeIdentity(size)))
			throw new NonSolvableException();
		double[][] solution = new double[augmented.rows][1];
		for (int i = 0; i < augmented.rows; i++)
			solution[i][0] = augmented.vals[i][augmented.cols - 1];
		return new Matrix(solution);
	}

	public boolean isSquare() {
		if (rows == cols)
			return true;
		else
			return false;
	}

	public static double trace(Matrix m) throws DimensionMismatchException {
		if (!m.isSquare())
			throw new DimensionMismatchException();
		double trace = 0;
		for (int i = 0; i < m.cols; i++)
			trace += m.vals[i][i];
		return trace;
	}

	public static Matrix scalarMult(Matrix m, double factor) {
		double[][] M = new double[m.rows][m.cols];
		for (int i = 0; i < m.rows; i++)
			for (int j = 0; j < m.cols; j++)
				M[i][j] = factor * m.vals[i][j];
		try {
			return new Matrix(M);
		} catch (DimensionMismatchException e) {
			return null;
		}
	}

	@Override
	public String toString() {
		String result = "{";
		for (int i = 0; i < rows; i++) {
			result += "{";
			for (int j = 0; j < cols; j++) {
				result += "\t"
						+ (Math.abs(vals[i][j] - (int) vals[i][j]) < 0.00001 ? ""
								+ (int) (vals[i][j])
								: Math.abs(vals[i][j] - (int) vals[i][j] - 1) < 0.00001 ? ""
										+ (int) (vals[i][j] + 1)
										: "" + vals[i][j]);
				if (j != cols - 1)
					result += ",";
			}
			result += "}";
			if (i != rows - 1)
				result += "\n";
		}
		result += "}";
		return result;
	}

	public static boolean equalsWithEp(double a, double b, double epsilon) {
		return Math.abs(a - b) <= epsilon;
	}

	public boolean equals(Object o) {
		if (o.getClass() != this.getClass())
			return false;
		Matrix m = (Matrix) o;
		if (rows != m.rows || cols != m.cols)
			return false;
		for (int i = 0; i < rows; i++) {
			for (int j = 0; j < cols; j++)
				if (vals[i][j] != m.vals[i][j])
					return false;
		}
		return true;
	}

	public static double hypot(double a, double b) {
		double r;
		if (Math.abs(a) > Math.abs(b)) {
			r = b / a;
			r = Math.abs(a) * Math.sqrt(1 + r * r);
		} else if (b != 0) {
			r = a / b;
			r = Math.abs(b) * Math.sqrt(1 + r * r);
		} else {
			r = 0.0;
		}
		return r;
	}

	public static String toFraction(double x) {

		return "";
	}

	public int getRows() {
		return rows;
	}

	public int getCols() {
		return cols;
	}

	public double[][] getVals() {
		return vals;
	}

	public static Matrix[] SVD(Matrix A) throws DimensionMismatchException {
		Matrix[] svd = new Matrix[3];

		Matrix aTransA = product(transpose(A), A);

//		Matrix[] jacobiResults = QRIteration(aTransA, 1e-2);
		Matrix[] jacobiResults = QRIteration(aTransA, 1e-8);

		svd[0] = jacobiResults[0];

		svd[1] = jacobiResults[1];
		
		System.out.println();

		System.err.println(svd[1]);

		double[][] sigInverseArr = new Matrix(svd[1]).getVals();
		for (int i = 0; i < sigInverseArr.length; i++) {
			if (sigInverseArr[i][i] != 0)
				sigInverseArr[i][i] = 1 / sigInverseArr[i][i];
		}
		Matrix sigInverse = new Matrix(sigInverseArr);

		System.err.println(transpose(A).rows + ", " + transpose(A).cols);
		System.err.println(svd[0].rows + ", " + svd[0].cols);
		System.err.println(sigInverse.rows + ", " + sigInverse.cols);

		svd[2] = product(transpose(A), svd[0]);
		svd[2] = product(svd[2], sigInverse);

		return svd;
	}
}
